import smbus
import time
import math
import numpy as np

PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A
GYRO_CONFIG  = 0x1B
INT_ENABLE   = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H  = 0x43
GYRO_YOUT_H  = 0x45
GYRO_ZOUT_H  = 0x47

CalGx = -13.4
CalGy = -48.55
CalGz = 5.37
CalAx = -809.336
CalAy = -461.796
CalAz = 1203.156

Aunit = 16384.0
Gunit = 131.0

F = 0.2
Ki = 0.01

def MPU_Init():
    bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
    bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
    bus.write_byte_data(Device_Address, CONFIG, 0)
    bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
    bus.write_byte_data(Device_Address, INT_ENABLE, 1)


def read_raw_data(addr):
    high = bus.read_byte_data(Device_Address, addr)
    low = bus.read_byte_data(Device_Address, addr + 1)
    value = ((high << 8) | low)
    if (value >= 0x8000):
        return -((65535 - value) + 1)
    else:
        return value
    return value

bus = smbus.SMBus(1)
Device_Address = 0x68
MPU_Init()

print (" Reading Data of Gyroscope and Accelerometer")

# for i in range(Calt):
#     time.sleep(0.01)
#     CalAx += read_raw_data(ACCEL_XOUT_H)
#     CalAy += read_raw_data(ACCEL_YOUT_H)
#     CalAz += read_raw_data(ACCEL_ZOUT_H)
#     CalGx += read_raw_data(GYRO_XOUT_H)
#     CalGy += read_raw_data(GYRO_YOUT_H)
#     CalGz += read_raw_data(GYRO_ZOUT_H)
# CalAx /= Calt
# CalAy /= Calt
# CalAz /= Calt
# CalGx /= Calt
# CalGy /= Calt
# CalGz /= Calt
time_pre = time.time()
attw = -1
attx = 0
atty = 0
attz = 0
exInt = 0
eyInt = 0
ezInt = 0
while True:
    acc_x = read_raw_data(ACCEL_XOUT_H) - CalAx
    acc_y = read_raw_data(ACCEL_YOUT_H) - CalAy
    acc_z = read_raw_data(ACCEL_ZOUT_H) - CalAz
    gyro_x = read_raw_data(GYRO_XOUT_H) - CalGx
    gyro_y = read_raw_data(GYRO_YOUT_H) - CalGy
    gyro_z = read_raw_data(GYRO_ZOUT_H) - CalGz
    dt = time.time() - time_pre

    Ax = acc_x/Aunit
    Ay = acc_y/Aunit
    Az = acc_z/Aunit
    S = math.sqrt(Ax ** 2 + Ay ** 2 + Az ** 2)
    Ax = Ax / S
    Ay = Ay / S
    Az = Az / S
    Gx = gyro_x/Gunit
    Gy = gyro_y/Gunit
    Gz = gyro_z/Gunit

    xac = 2 * (attx * attz - attw * atty)
    yac = 2 * (atty * attz + attw * attx)
    zac = 1 - 2 * attx ** 2 - 2 * atty ** 2

    xca = Ay * zac - Az * yac
    yca = Az * xac - Ax * zac
    zca = Ax * yac - Ay * xac

    exInt = exInt + xca * Ki
    eyInt = eyInt + yca * Ki
    ezInt = ezInt + zca * Ki

    dx = Gx + xca * F + exInt
    dy = Gy + yca * F + eyInt
    dz = Gz + zca * F + ezInt

    attw -= (attx * dx + atty * dy + attz * dz) * dt / 2
    attx += (attw * dx + atty * dz + attz * dy) * dt / 2
    atty += (attw * dy + attx * dz + attz * dx) * dt / 2
    attz += (attw * dz + atty * dx + attx * dy) * dt / 2

    attl = math.sqrt(attw ** 2 + attx ** 2 + atty ** 2 + attz ** 2)
    attw /= attl
    attx /= attl
    atty /= attl
    attz /= attl
    print(ezInt, Gx, Gy, Gz)
    print(attw, attx, atty, attz)
    print()
